% opt2surf2
% Christopher J. Whalen
% University of Illinois
% 05/27/05 - Date Created
% 05/27/05 - Last Modified

% Purpose - to replace the elp point location with the location of the
% closest scalp point

function [xyz] = opt2surf2 (xyz,xyz_mask)

global index % make index into mask available (4 writing .stl file)

% fprintf('\nopt2surf2');
% fprintf('\nScalp Forcing');
% fprintf('\nClosest Point\n');
% fprintf('Estimated Time: 1.0 min\n');

% hold on;
% scatter3(xyz(:,1),xyz(:,2),xyz(:,3),'b');
% hold on;

xyz_dist = zeros(size(xyz_mask,1),1); % preallocating 
min_dist = zeros(size(xyz,1),1);
index = zeros(size(xyz,1),1);
tic;

% !!! this brute force approach is actually pretty fast,
% but could be replaced with a kdtree search...  elm
%
for i=1:size(xyz,1)
    xyz_repmat = repmat(xyz(i,:),size(xyz_mask,1),1);
    xyz_diff = xyz_mask - xyz_repmat;
    xyz_dist(:) = xyz_diff(:,1).^2 + xyz_diff(:,2).^2 + xyz_diff(:,3).^2;
    [min_dist(i,1),index(i,1)] = min(xyz_dist(:)); % finding the nearest point
    index(i,1) = index(i,1);
    xyz(i,:)=xyz_mask(index(i,1),:);
end
% min_dist;

%scatter3(xyz(:,1),xyz(:,2),xyz(:,3),'g','filled');


% fprintf('\nActual Time: %2.1f min \n',toc/60);
% fprintf('     opt2surf2 done\n');


% Error in X vs X
%plot(xyz(:,1),xyz(:,1)-xyz_b4_force(:,1));




